#ifndef _ALGO_FILTER_H
#define _ALGO_FILTER_H

#include "ct_type.h"
#include "algo_qfp16.h"
//-------------------------------------
// 1. low-pass filter
//-------------------------------------
/**
 * @brief Apply a low-pass filter to the input sample based on a percentage weight.
 * @param output The current filter output; will be updated with the new filtered value.
 * @param new_sample The new sample to be filtered.
 * @param percentTimes256 The weight of the output, as a percentage times 256.
 */
void LPF1_Standard(int16_t *output, int16_t new_sample, uint8_t percentTimes256);

/**
 * @brief Apply a low-pass filter to the input sample based on a percentage weight.
 *
 * @param output The current filter output; will be updated with the new filtered value.
 * @param new_sample The new sample to be filtered.
 * @param percent The weight of the output, as a percentage.
 */
void LPF1_FP16(float *output, float new_sample, float percent);

//-------------------------------------
// 1.1 (second-order low-pass filter)
//-------------------------------------
typedef struct LPF2_State_s {
    qFP16_t x1;      // Previous input
    qFP16_t x2;      // Input two times ago
    qFP16_t y1;      // Previous output
    qFP16_t y2;      // Output two times ago
    qFP16_t k;       // Coefficient of the current input
    qFP16_t b1;      // Coefficient of the previous input
    qFP16_t b2;      // Coefficient of the input two times ago
    qFP16_t a1;      // Coefficient of the previous output
    qFP16_t a2;      // Coefficient of the output two times ago
    qFP16_t init;    // Initialization flag
} LPF2_State_t;
/**
 * @brief Initialize the LPF2 state with a given alpha value.
 *
 * @param state Pointer to the LPF2 state structure.
 * @param input_alpha The alpha value used for initialization.
 * @return 0 if successful, -1 if the parameters are invalid.
 */
int init_LPF2_FP16(LPF2_State_t *state, float input_alpha);

/**
 * @brief Apply the LPF2 filter to a new sample.
 *
 * @param state Pointer to the LPF2 state structure.
 * @param new_sample The new sample to be filtered.
 * @return The filtered sample.
 */
int16_t LPF2_FP16(LPF2_State_t *state, int16_t new_sample);

//-------------------------------------
// 2.1 Sliding Window Filter
//-------------------------------------

typedef struct {
    int32_t sum;
    uint8_t shift;
    uint8_t init;
} SW_Filter_t;
// Initialize the filter
void init_SW_filter(SW_Filter_t *state, uint8_t shift);
// Simplified sliding window filter function
int16_t SW_filter(SW_Filter_t *state, int16_t new_value);

//-------------------------------------
// 2.2 Moving average filter
//-------------------------------------

typedef struct {
    int16_t *buffer;    // Pointer to static array
    int32_t  sum;
    uint8_t  index;        // Use a smaller data type to store the index
    uint16_t buf_size;     // Buffer size
    uint8_t  buf_shift;    // Bit shift for calculating the average
    uint8_t  init;         // Initialization flag
} MW_Filter_t;

// Initialize the filter, size and shift need to be defined elsewhere
void init_moving_average(MW_Filter_t *state, int16_t *filterBuffer, uint16_t size, uint8_t shift);
//  Moving Average Filter (reentrant version)
int16_t moving_average_filter(MW_Filter_t *state, int16_t dat);


//-------------------------------------
// 2.3 Weighted moving average filter (reentrant version)
//-------------------------------------
typedef struct {
    int16_t *buffer;    // Pointer to static array
    int16_t *weight;    // Pointer to weight array, value range [0,255]
    int32_t  sum;
    uint16_t index;        // Use a smaller data type to store the index
    uint16_t buf_size;     // Buffer size
    uint8_t  buf_shift;    // Bit shift for calculating the average
    uint8_t  init;         // Initialization flag
} weighted_MW_Filter_t;

/**
 * @brief  Initialize the weighted moving average filter
 * @param  state: pointer to the filter state structure
 * @param  filterBuffer: pointer to the filter buffer
 * @param  weight: pointer to the weight factor array
 * @param  size: size of the filter buffer
 * @param  shift: shift amount for the filter
 * @retval None
 */
void weighted_init_moving_average(weighted_MW_Filter_t *state, int16_t *filterBuffer, int16_t *weight,
                                  uint16_t size, unsigned char shift);
/**
 * @brief  Weighted moving average filter (reentrant version)
 * @param  state: pointer to the filter state structure
 * @param  new_value: new input value
 * @retval filtered value
 */
int16_t weighted_moving_average_filter(weighted_MW_Filter_t *state, int16_t dat);

//-------------------------------------
// 2.4 Moving Outlier Removal (MOR) Filter
//-------------------------------------

typedef struct MOR_Filter_s {
    int16_t *buffer;
    uint16_t buf_size;
    uint8_t  buf_shift;
    int16_t  median;
    int16_t  threshold;
    int32_t  sum;
    uint16_t index;
    uint8_t  init;
} MOR_Filter_t;

/**
 *  @brief Initialize the Moving Outlier Removal (MOR) filter
 *  @param[in] state Pointer to the filter state structure
 *  @param[in] buffer Pointer to the filter buffer array
 *  @param[in] buf_size Size of the filter buffer array
 *  @param[in] shift Shift amount for the filter buffer array
 *  @param[in] threshold Threshold for the filter
 *  @return None
 */
void init_mor_filter(MOR_Filter_t *state, int16_t *buffer, uint16_t buf_size, uint8_t shift, int16_t threshold);
/**
 *  @brief Update the Moving Outlier Removal (MOR) filter
 *  @param[in] state Pointer to the filter state structure
 *  @param[in] new_value New value to be filtered
 *  @return Filtered value
 */
int16_t mor_filter_update(MOR_Filter_t *state, int16_t new_value);

//-------------------------------------
// 3. Median Filter
//-------------------------------------
typedef struct {
    int16_t *buffer;
    uint16_t size;
    uint16_t next_index;
    uint8_t  init;
} MedianFilte_t;

/**
 * @brief  Median Filter initialization function
 * @param  state: MedianFilte_t structure
 * @param  buffer: Pointer to the external array
 * @param  size: Size of the external array
 * @retval None
 */
void init_MedianFilter(MedianFilte_t *state, int16_t *buffer, uint16_t size);

/**
 * @brief  Median Filter function
 * @param  state: MedianFilte_t structure
 * @param  new_sample: New sample to be added to the filter
 * @retval Median value
 */
int16_t MedianFilter(MedianFilte_t *state, int16_t new_sample);




//-------------------------------------
// 4. Scalar Kalman Filter
//-------------------------------------
typedef struct {

    qFP16_t x;    /* state */
    qFP16_t P;    /* estimated error covariance */
    qFP16_t A;    /* x(n)=A*x(n-1)+u(n),u(n)~N(0,Q) */
    qFP16_t H;    /* z(n)=H*x(n)+w(n),w(n)~N(0,R) */
    qFP16_t Q;    /* process(predict) noise covariance */
    qFP16_t R;    /* measure noise covariance */
    qFP16_t gain; /* Kalman gain */
    uint8_t init; /*initialization flag*/
} KalmanParams_t;

/**
 * @brief Initialize the Kalman filter
 * @param state Pointer to the filter state structure
 * @param initial_x The initial state value
 * @param initial_p The initial error covariance
 * @param A The state transition matrix
 * @param H The measurement matrix
 * @param Q The process noise covariance
 * @param R The measurement noise covariance
 */
void KalmanFilter_Init(KalmanParams_t *const s, float initial_x, float initial_p, float A, float H,
                       float Q, float R);
/**
 * @brief Perform a Kalman filter update for a one-dimensional
 * process
 * @param kalmanParams Pointer to the Kalman filter parameters
 * @param measuredValue The new measured value
 * @return The updated estimated value as int16_t
 * @note The quotient mapped from [0,1] to the range [0, 256]
 */
int16_t KalmanFilter(KalmanParams_t *const s, const int16_t measuredValue);

//-------------------------------------
// 5. Gaussian Filter
//-------------------------------------
typedef struct {
    qFP16_t *window;
    qFP16_t *kernel;
    uint16_t window_size;
    uint8_t  init;
} qSSmoother_GMWF_t;

/**
 * @brief Initialize the Gaussian Moving Window filter
 * @param state Pointer to the filter state structure
 * @param params Pointer to the filter parameters
 * @param window Pointer to the window array
 * @param window_size Size of the window array
 * @return 0 on success, -1 on error
 * @note The window array must be allocated by the caller
 */
int8_t init_GMWFilter(qSSmoother_GMWF_t *state, const qFP16_t *params, qFP16_t *window, uint16_t window_size);


/**
 * @brief Apply the Gaussian Moving Window filter to the input value.
 *
 * @param state Pointer to the filter state structure.
 * @param x The input value.
 * @return int16_t The filtered value.
 */
int16_t GMWFilter(qSSmoother_GMWF_t *state, int16_t x);


//-------------------------------------
// 6.1 Exponential Smoothing Filter
//-------------------------------------

/* ExpFilter_t structure, used for exponential weighted moving average filtering */
typedef struct {
    qFP16_t lambda; /* Exponential decay coefficient */
    qFP16_t m;      /* Filtered value */
    qFP16_t w;      /* Weight */
    uint8_t init;
} ExpFilter_t;

/**
 * @brief Set up the ExpFilter_t structure with the specified lambda value.
 *
 * @param s Pointer to the ExpFilter_t structure.
 * @param lambda The lambda value for exponential decay.
 * @return int Returns 1 if successful, 0 otherwise.
 */
int init_ExpFilter(ExpFilter_t *const s, const float lambda);

/**
 * @brief Filter the input value using exponential weighted moving average.
 *
 * @param s Pointer to the ExpFilter_t structure.
 * @param x The input value to be filtered.
 * @return qFP16_t Returns the filtered value.
 */
qFP16_t ExpFilter(ExpFilter_t *const s, const int16_t x);


//-------------------------------------
// 6.2 Double Exponential Smoothing Filter
//-------------------------------------

typedef struct {
    uint8_t init;  /**< Flag indicating whether the filter has been initialized. */
    qFP16_t alpha; /**< Filter parameter alpha. */
    qFP16_t beta;  /**< Filter parameter beta. */
    qFP16_t n;     /**< Filter parameter n. */
    qFP16_t lt;    /**< Filter state variable lt. */
    qFP16_t bt;    /**< Filter state variable bt. */
} DESFilter_t;

/**
 * @brief Sets up the DES filter with the given parameters.
 *
 * @param s Pointer to the DES filter structure.
 * @param param Array of three float values representing the alpha, beta, and n parameters
 * of the filter.
 *              - param[0]: Generalized autoregressive coefficient (a), range: (0, 1).
 *              - param[1]: Generalized moving average coefficient (b), range: (0, 1).
 *              - param[2]: Moving average order (n), must be greater than 0.
 * @return int16_t Returns 1 if the filter was set up successfully, 0 otherwise.
 */
int16_t init_DoubleExpFilter(DESFilter_t *const s, const float *const param);

/**
 * @brief Applies the DES filter to the input signal.
 *
 * @param s Pointer to the DES filter structure.
 * @param x The input signal value.
 * @return int16_t The filtered output signal value.
 */
int16_t DoubleExpFilter(DESFilter_t *const s, const int16_t x);


#endif    // __ALGO_FILTER_H__
